/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Robot;

import lejos.robotics.subsumption.Behavior;

/**
 *
 * @author zach
 */
public class CheckConditions implements Behavior{

    public boolean takeControl() {
        return Configureation.ultraSoundSensor().getDistance()<100f;
    }

    public void action() {
        Configureation.getSensorMotors()[0].rotate(5820/3);
        int left =Configureation.ultraSoundSensor().getDistance();
        Configureation.getSensorMotors()[0].rotate(5820/3);
        Configureation.getSensorMotors()[0].rotate(5820/3);
        int fullLeft =Configureation.ultraSoundSensor().getDistance();
        Configureation.getSensorMotors()[0].rotate(-5820/3);
        Configureation.getSensorMotors()[0].rotate(-5820/3);
        Configureation.getSensorMotors()[0].rotate(-5820/3);
        int center =Configureation.ultraSoundSensor().getDistance();
        Configureation.getSensorMotors()[0].rotate(-5820/3);
        int right =Configureation.ultraSoundSensor().getDistance();
        Configureation.getSensorMotors()[0].rotate(-5820/3);
        Configureation.getSensorMotors()[0].rotate(-5820/3);
        int fullRight =Configureation.ultraSoundSensor().getDistance();

        if(left/Math.sqrt(3)*2==center||right/Math.sqrt(3)*2==center){
            Constants.conditionCode=Constants.isWall;
        }
        else if(fullLeft<=100&&fullRight<=100){
            Constants.conditionCode=Constants.isTarp;
        }else if(fullLeft<=100){
            Constants.conditionCode=Constants.isLCorner;
        }else if(fullRight<=100){
            Constants.conditionCode=Constants.isRconner;
        }else{
            Constants.conditionCode=Constants.isObstical;
        }

    }

    public void suppress() {
    }


}
